#include "EvalUtil.h"
#include <SolverUtil.h>
#include <BEMUtil.h>

extern SurfTrack* surf;

void assemblyBoundaryVector(SurfTrack* surf, double** b)
{
	//int nTri = surf->m_mesh.num_triangles();

	int nEdg = surf->m_mesh.edges.size();

	vectorClear(b, nEdg * 2);

	for (int i = 0; i<nEdg; i++)
	{
		Vec2ui qe = surf->m_mesh.edges[i];

		Vec2d q0 = surf->m_positions[qe[0]];
		Vec2d q1 = surf->m_positions[qe[1]];
		

		if ((q0[0] <-1 + 1e-5) && (q1[0] <-1 + 1e-5))
		{
			(*b)[2 * i + 0] = -3;
		}

		if ((q0[0] > (1 - 1e-5)) && (q1[0] > (1 - 1e-5)))
		{
			(*b)[2 * i + 0] = 3;
		}
	}

	
}


void myMatrixVectorProduct(double* x, double** y, int n)
{
	int nEdg = n / 2;
	//double* A = (double*)malloc(3 * n * sizeof(double));

	vectorAmplititude(x, 0.5, y, n);
	//vectorClear(y, n);
	//vectorClear(&A, 3*n);

#pragma omp parallel for
	for (int i = 0; i<nEdg; i++)
	{
		Vec2ui e = surf->m_mesh.edges[i];

		Vec2d p0 = surf->m_positions[e[0]];
		Vec2d p1 = surf->m_positions[e[1]];
		

		Vec2d p = (p0 + p1) / 2.0;

		for (int j = 0; j<nEdg; j++)
		{
			Vec2ui qe = surf->m_mesh.edges[j];

			Vec2d q0 = surf->m_positions[qe[0]];
			Vec2d q1 = surf->m_positions[qe[1]];
			

			double T = surf->get_edge_length(j);

			Vec2d qn = surf->get_edge_normal(j);
			

			Mat22d v0 = evaluateGradientQuadraturePoint(p, q0, qn);
			Mat22d v1 = evaluateGradientQuadraturePoint(p, q1, qn);
			

			Vec2d vx(x[2 * j + 0], x[2 * j + 1]);
			Vec2d vy = (v0 + v1) * T / 2.0 * vx;

			(*y)[2 * i + 0] += vy[0];
			(*y)[2 * i + 1] += vy[1];			

		}
	}
}


void evaluateDisplacementField(double* theta, double** u)
{
	int nEdg = surf->m_mesh.edges.size();
	int nVer = surf->m_positions.size();
	int n = nVer * 2;

	vectorClear(u, n);

#pragma omp parallel for
	for (int i = 0; i<nVer; i++)
	{
		Vec2d p = surf->m_positions[i];

		for (int j = 0; j<nEdg; j++)
		{
			Vec2ui qe = surf->m_mesh.edges[j];

			Vec2d q0 = surf->m_positions[qe[0]];
			Vec2d q1 = surf->m_positions[qe[1]];

			double T = surf->get_edge_length(j);

			Mat22d v0 = evaluateQuadraturePoint(p, q0);
			Mat22d v1 = evaluateQuadraturePoint(p, q1);			

			Vec2d vx(theta[2 * j + 0], theta[2 * j + 1]);
			Vec2d vy = (v0 + v1) * T / 2.0 * vx;

			(*u)[2 * i + 0] += vy[0];
			(*u)[2 * i + 1] += vy[1];
			
		}
	}
}


void evaluateDeformationGradientField(double* theta, double** F)
{
	int nEdg = surf->m_mesh.edges.size();
	int nVer = surf->m_positions.size();
	int n = 4 * nVer;

	//vectorClear(F, n);

	double K[2][2][2];

#pragma omp parallel for
	for (int i = 0; i<nVer; i++)
	{
		Vec2d p = surf->m_positions[i];

		Mat22d Ft(0, 0, 0, 0);

		for (int j = 0; j<nEdg; j++)
		{
			Vec2ui qe = surf->m_mesh.edges[j];

			Vec2d q0 = surf->m_positions[qe[0]];
			Vec2d q1 = surf->m_positions[qe[1]];
			

			double T = surf->get_edge_length(j);

			double* v0 = getKernelGradient(p, q0);
			double* v1 = getKernelGradient(p, q1);
			

			for (int i = 0; i<2; i++)
			{
				for (int j = 0; j<2; j++)
				{
					for (int k = 0; k<2; k++)
					{
						K[i][j][k] = (v0[getIndex(i, j, k)] + v1[getIndex(i, j, k)]) * T / 2.0;
					}
				}
			}

			free(v0);
			free(v1);
			

			Vec2d s(theta[2 * j + 0], theta[2 * j + 1]);

			Mat22d Gu = tensorVectorContraction(K, s);

			Ft += Gu + Gu.transpose();
		}

		for (int a = 0; a<3; a++)
		{
			for (int b = 0; b<3; b++)
			{
				(*F)[getTensorArrayIndex(a, b, i)] = Ft(a, b);
			}
		}

	}
}